%% 一阶卡尔曼滤波与低通滤波器对比
% 初始化参数
ts = 0.001;                 % 采样时间
delta_t  = 0.02;            % 实际信号帧率
noise_mag = 1;              % 设置噪声幅度为1
t = 0:ts:2-ts;
t2 = 0:delta_t:2-delta_t;
N = length(t);              % 序列的长度
sz = [1,N];                 % 信号需开辟的内存空间大小 
x = 20*sin(2*pi*t);         % 真实位置
noise = noise_mag*randn(1,N*ts/delta_t);              % 测量白噪声
z = interp1 (t2,20*sin(2*pi*t2)+noise,t,'previous');  % 信号叠加噪声后重新采样
Q = 1;                      % 假设建立的模型噪声方差
R = 2000;                   % 位置测量方差估计,Q/R减小会增加延迟，但是更平滑

A=1;
B=0;
H=1;

n=size(Q);  
m=size(R);

% 分配空间
xhat=zeros(sz);         % 后验估计
P=0;                    % 后验方差估计  
xhatminus=zeros(sz);    % 先验估计
Pminus=zeros(n);        % 先验方差估计
K=zeros(n(1),m(1));     % Kalman增益
I=eye(n);

% 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0
for k = 2:N
    % 时间更新过程
    xhatminus(:,k) = A*xhat(:,k-1)+B;
    Pminus= A*P*A'+Q;
    
    % 测量更新过程
    K = Pminus*H'/( H*Pminus*H'+R );
    xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));
    P = (I-K*H)*Pminus;
end
F = filter_custom;
z_lp = filter(F,z);
x_k = cat(1,x(1:1950),xhat(1:1950));
distance_k = pdist(x_k); % 计算两个向量的欧氏距离作为偏差的判定
x_lp = cat(1,x(1:1950),z_lp(1:1950));
distance_lp = pdist(x_lp);
x_z = cat(1,x(1:1950),z(1:1950));
distance_z = pdist(x_z);

figure;
plot(t,x(1,:),'g-');
hold on
plot(t,z);
plot(t,xhat(1,:),'r-')
plot(t,z_lp,'k-');
legend('truth',['measurement ',num2str(distance_z)], ['kalman filter ',num2str(distance_k)], ['lowpass ',num2str(distance_lp)]);
xlabel('time(s)');

function Hd = filter_custom
% Chebyshev Type II Lowpass filter designed using FDESIGN.LOWPASS.

% All frequency values are in Hz.
Fs = 1000;  % Sampling Frequency

N     = 10;  % Order
Fstop = 20;  % Stopband Frequency
Astop = 80;  % Stopband Attenuation (dB)

% Construct an FDESIGN object and call its CHEBY2 method.
h  = fdesign.lowpass('N,Fst,Ast', N, Fstop, Astop, Fs);
Hd = design(h, 'cheby2');
end